#include "componentSelection.h"

void
segmentCluster
(
    PointIndices::Ptr output,
    vector<PointIndices> cluster_indices_out,
    int selectedPointIndex
)
{
    //cout << "segmentCluster... " << flush; // DEBUG
//    i cluster index
//    j index of the point in the i-th cluster
//    if the j-th point is the one selected the cluster selected is the i-th
    int selectedClusterIndex=-1;
    for (int i=0;i<cluster_indices_out.size();i++)
        if (selectedClusterIndex>0) break;
        else
            for (int j=0;j<cluster_indices_out[i].indices.size();j++)
                if(cluster_indices_out[i].indices.at(j)==selectedPointIndex)
                {
                    selectedClusterIndex=i;
                    break;
                }

    //non sono riuscito a tirar fuori il cluster in altro modo, ma si dovrebbe riuscire a fare meglio...
    PointIndices temp=cluster_indices_out[selectedClusterIndex];
    while(!temp.indices.empty())
    {
        output->indices.push_back(temp.indices.back());
        temp.indices.pop_back();
    }
}

void segmentClusterNearestPoint(PointCloud<PointXYZRGB>::Ptr input,
                                PointIndices::Ptr output,
                                vector<PointIndices> cluster_indices_out,
                                PointXYZRGB clicked)
{
    //    i cluster index
    //    j index of the point in the i-th cluster
    //    if the j-th point is the one selected the cluster selected is the i-th
        int selectedClusterIndex=-1;
        float minDist=FLT_MAX;
         for (int i=0;i<cluster_indices_out.size();i++)//per ogni cluster
             for (int j=0;j<cluster_indices_out.at(i).indices.size();j++)//per ogni punto-indice del cluster
             {
                 PointXYZRGB pointOnSource=input->at(cluster_indices_out.at(i).indices.at(j));
                 float dist = (pointOnSource.getVector3fMap()-clicked.getVector3fMap()).norm();
                    if(dist<minDist)
                    {
                        selectedClusterIndex=i;
                        minDist=dist;
                    }
            }
          //non sono riuscito a tirar fuori il cluster in altro modo, ma si dovrebbe riuscire a fare meglio...
        PointIndices temp=cluster_indices_out[selectedClusterIndex];
        while(!temp.indices.empty())
        {
            output->indices.push_back(temp.indices.back());
            temp.indices.pop_back();
        }

}


void segmentColor
(
    PointCloud<PointXYZRGB>::Ptr input,
    PointIndices::Ptr output,
    int selectedPointIndex,
    int threshold
)
{
    //cout << "segmentColor... " << flush; // DEBUG
    PointXYZRGB selectedPoint = (*input)[selectedPointIndex];
    int r = selectedPoint.r;
    int g = selectedPoint.g;
    int b = selectedPoint.b;
    int i = 0;
    for (i = input->size()-1; i >0 ; i--)//Al contrario per mantenere ordinati in modo crescente gli indici
    {
        if
        (
             abs(input->at(i).r-r)<threshold &&
             abs(input->at(i).g-g)<threshold &&
             abs(input->at(i).b-b)<threshold
        )
        output->indices.push_back(i);

    }

}


void
intersectIndices
(
    PointIndices::Ptr first,
    PointIndices::Ptr second,
    PointIndices::Ptr intersection
)
{
    //cout << "intersectIndices... " << flush; // DEBUG

    // point indices intersection cycle
    while(!first->indices.empty() && !second->indices.empty())
    {
        if(first->indices.back()==second->indices.back())//trovato
        {
            intersection->indices.push_back(first->indices.back()); //pusha dentro
            first->indices.pop_back(); //passa al prossimo
        }
        else //poppa fuori il minimo
        {
            if(first->indices.back()  <  second->indices.back())
                first->indices.pop_back();
            else
                second->indices.pop_back();
        }
    }

}



